#include<iostream>
#include<algorithm>
#include<fstream>
#include<string>

#include"opencv2/core/core.hpp"
#include <unistd.h>

#include "coarseLocating.hpp"
using namespace std;
using namespace cv;
using namespace xyVision;

void LoadImages(const string& strSequence, vector<string>& vstrImageFilenames,
                vector<double> &vTimestamps, vector<vector<double> >& gpses);

int main()
{
    
    xyVision::coarseLocating sample("config_sq1.ini");
    cv::VideoWriter writer("gps_result.avi",CV_FOURCC('M','J','P','G'), 10, Size(640, 360));

    string imgPathTxt = "seq1_gps_list.txt";
    string strSequence(imgPathTxt);
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    vector<vector<double> > gpses;
    LoadImages(strSequence, vstrImageFilenames, vTimestamps, gpses);
    int nImages = (int)vstrImageFilenames.size();
    Mat img;
    cout.precision(10);
    for(int ni=0; ni<nImages; ni++) {
        img = imread(vstrImageFilenames[ni]);
        sample.getCurrentGPS(img);

        cout << "Locating frame " << ni << endl;
        if (sample.isDetected) {
            vector<double> detGPS = sample.gps;
            //cout << "Size of GPS" << (int)detGPS.size() << endl;
            cout << "Detected GPS " << detGPS[0] << " " << detGPS[1] << endl;
            cout << "Ground Truth GPS" << gpses[ni][0] << " " << gpses[ni][1] << endl;
        } else {
            cout << "Frame " << ni << " does not detected" << endl;
        }
    }    

}

void LoadImages(const string& strSequence, vector<string>& vstrImageFilenames,
                vector<double> &vTimestamps, vector<vector<double> >& gpses)
{
    vstrImageFilenames.resize(0);
    vTimestamps.resize(0);
    gpses.resize(0);
    double time = 0.0;
    ifstream fs(strSequence.c_str());
    if(!fs.is_open())
    {
        cout << "Error opening file" <<endl;
    }
    char buffer[512];
    char file_name[512];
    double lat, lon;

    while(!fs.eof())
    {
        fs.getline(buffer, 512);
        sscanf(buffer, "%[^,],%lf,%lf", file_name, &lat, &lon);
        vstrImageFilenames.push_back(string(file_name));
        vTimestamps.push_back(time);
        vector<double> gps;
        gps.push_back(lat);
        gps.push_back(lon);
        gpses.push_back(gps);
        time = time + 1.0;
    }
}
